/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file ipmt.cxx
  \brief class implemenation for IP media test tools

  \author Cory Dixon
*/
#include <iostream>
#include "ipmt.h"
#include "utilities.h"
#include "color.h"

/*<-------Defines--------->*/
// this is 18 bytes smaller than standar 1500 byte MTU for TCP packets
#define PACKET_SIZE 1472
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
extern bool logging;
extern String logDir;
/*<-End Global Variables-->*/

IPMT::IPMT(String name) : ThreadedPipe(name)
{
	cout << GREEN << "---------->>>>>      " YELLOW "IPMT" GREEN "       <<<<<----------" << ATTROFF << endl;

	/*--------- log ----------*/
	if(logging) {
		log_name = generteNumberedFile( logDir, "ipmt_");
	}

	client_sock = NULL;
	udp_server_sock = NULL;
	tcp_server_sock = NULL;
	test_type = NO_TEST;
	test_time = 0;
}

IPMT::~IPMT()
{
	if( client_sock ) delete client_sock;
	if( udp_server_sock ) delete udp_server_sock;
	if( tcp_server_sock ) delete tcp_server_sock;
}

bool IPMT::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);

	udp_portStr  = "1212";
	tcp_portStr  = "1213";

	udp_server_sock = new Socket("localhost", udp_portStr.c_str(), Socket::UDP);
	if( udp_server_sock == NULL ) {
		cout << getName() << "::init() - could not get new UDP socket, " <<  strerror(errno) << endl;
	} else
		udp_server_sock->setNonBlocking();

	tcp_server_sock = new Socket("localhost", tcp_portStr.c_str(), Socket::TCP);
	if( tcp_server_sock == NULL ) {
		cout << getName() << "::init() - could not get new TCP socket, " <<  strerror(errno) << endl;
	} else
		tcp_server_sock->setNonBlocking();

	currentStamp.stamp();
	loop_ts = currentStamp;

	return true;
}

void IPMT::update() 
{
	// set run rate to be continuous
	if( execRate != 0 ) {
		setRunning(false);
		itPtr->stop();
		execRate = 0;
		itPtr->setRunRate(execRate);
		setRunning(true);

		cout << getName() << " -- TCP/UDP server sockets:" << endl;
	}

	struct timeval timeout;
	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = 1;//TIME_OUT_USEC;

	Client * me = shmPtr->clients.getClient(0);
	bool haveGPSupdate = false;

	// open TCP socket
	if( tcp_server_sock && tcp_server_sock->isClosed() ) {
		cout << "\t";
		if ( !tcp_server_sock->open( Socket::SERVER ) ) 
			cout << getName() << " !! could not open TCP server sock" << endl;
	}

	// open UDP socket
	if( udp_server_sock && udp_server_sock->isClosed() ) {
		cout << "\t";
		if ( !udp_server_sock->open( Socket::SERVER ) ) 
			cout << getName() << " !! could not open UDP server sock" << endl;
	}

	// set file descriptors for select
	int val, maxFD;
	fd_set readFDs, writeFDs;                  // fd sets
	FD_ZERO(&readFDs);                         // zero read fd set
	FD_ZERO(&writeFDs);                        // zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);     // add fifo
	maxFD = fifo_FDs[INBOARD_RX];

	int maxFD1 = tcp_server_sock->setFD(readFDs);
	if( maxFD1 > maxFD ) maxFD = maxFD1;
	int maxFD2 = udp_server_sock->setFD(readFDs);
	if( maxFD2 > maxFD ) maxFD = maxFD2;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("IPMT::update() select");
				execRate = -1;
			} //else cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {

		// UDP Server socket
		if( udp_server_sock ) {
			Socket::SocketWait status = udp_server_sock->checkFD(readFDs);

			if( status >= Socket::WAIT_DATA ) {
				// UDP Socket server
				int n = 0;
				while( (n=udp_server_sock->read((char *)fifo_msg.data, FIFO_BUF_SIZE)) >= 0 ) {
					//cout << getName() << " -- UDP received " << n << " bytes" << endl;
					n_rx += n;
					nb_rx++;
				}
			}
		}

		// TCP Server socket
		if( tcp_server_sock ) {
			Socket::SocketWait status = tcp_server_sock->checkFD(readFDs);
			if( (status & Socket::WAIT_PEER) > 0 ) {
				//cout << "connection request from tcp ... ";
				int cInd = -1;
				cInd = tcp_server_sock->connectClient();
				//cout << "connected client " << cInd << endl;
			} else if( (status & 255) > 0 ) {
				for(int i = 0; i < tcp_server_sock->getNumClients(); i++) {
					if(status & (0x1 << i) ) {            // check if FD is active
						int n = tcp_server_sock->read(i,(char *)fifo_msg.data, FIFO_BUF_SIZE);
						n_rx += n;
						nb_rx++;
						//cout << getName() << " -- TCP received " << n << " bytes from client " << i << endl;
						if( n == 0 ) {
							//cout << getName() << " -- removing client " << i << endl;
							tcp_server_sock->removeClient(i);
						}
					}
				}
			}
		}

		// Commands
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) 
		{
			//printf("INBOARD_RX: ");
			int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ){
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					// client that sent the message
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);
					if(a_client != NULL) {

						uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

						switch(cmd.cmd() ) 
						{
							case CMD_RATE_TEST: {
								RateTest_t *test = (RateTest_t *)dataPtr;

								test_type = (TestType)test->type;
								test_time = test->time;

								cout << "IPMT: CMD_RATE_TEST from " << a_client->name << endl
									<< "\tType=" << (int)test->type << " Time=" << (int)test->time << endl;

								switch( test_type ) {
									case NO_TEST:
										if( client_sock && !client_sock->isClosed() ) 
											client_sock->close();
										target_host = "";

										break;
									case UDP_TEST: 
										target_host = inet_ntoa(fifo_msg.src);
										cout << "\tUDP_TEST: host=" << target_host << endl;

										if( client_sock ) delete client_sock;
										client_sock = new Socket(target_host.c_str(), udp_portStr.c_str(), Socket::UDP);
										if( client_sock == NULL ) {
											cout << getName() << "::init() - could not get new socket, " <<  strerror(errno) << endl;
										} else
											client_sock->setNonBlocking();

										if ( client_sock && client_sock->open( Socket::CLIENT ) ) 
											cout << getName() << " --  UDP client socket is open" << endl;
										else
											cout << getName() << " !! could not open UDP client sock" << endl;

										break;
									case TCP_TEST:
										target_host = inet_ntoa(fifo_msg.src);
										cout << "\tTCP_TEST: host=" << target_host << endl;

										if( client_sock ) delete client_sock;
										client_sock = new Socket(target_host.c_str(), tcp_portStr.c_str(), Socket::TCP);
										if( client_sock == NULL ) {
											cout << getName() << "::init() - could not get new socket, " <<  strerror(errno) << endl;
										} else
											client_sock->setNonBlocking();

										if ( client_sock && client_sock->connectHost( ) ) 
											cout << getName() << " --  TCP client socket is open" << endl;
										else
											cout << getName() << " !! could not open TCP client sock" << endl;

										break;
								}

								ACK(INBOARD_TX);
								break;
							} 

							case CMD_TELEM: 
								// set gps flag to update controller
								haveGPSupdate = true;
								break;

							default:
								//printf("--> IPMT UNHANDLED CMD: cmd=0x%2x\n",cmd.cmd());
								break;
						}
					}
				}
			}
		}
	}

	// UDP/TCP Client sends the data
	if( test_type > NO_TEST && client_sock ) {
		//cout << getName() << " -- sending PACKET_SIZE bytes" << endl;
		n_tx += client_sock->write(0, (char *)fifo_msg.data, PACKET_SIZE);
		nb_tx++;
	}

	// collect stats
	currentStamp.stamp();
	float dT = (currentStamp - loop_ts) / SEC2MICRO;
	if( dT >= 1.0 ) {
		loop_ts = currentStamp;

		rate_rx = n_rx / dT / 1024.0; //1048576.0;
		n_rx    = 0;
		rate_tx = n_tx / dT / 1024.0; //1048576.0;
		n_tx    = 0;

		me->comm[0].rx_rate = rate_rx;
		me->comm[0].tx_rate = rate_tx;

		if( test_type > 0 )
		{
			printf("%s -- TX=%10.1f [kB/s] RX=%10.1f [kB/s]\n", getName().c_str(), rate_tx, rate_rx);

			//-- log the data
			if(logging) 
			{
				logfile=fopen(log_name.c_str(),"a");
				if(logfile != NULL) {
					update_gps_str();
					fprintf(logfile,"%s\n", gpsStr);

					fprintf(logfile,"%s %i %f %f\n",
							target_host.c_str(),
							test_type,
							me->comm[0].tx_rate = rate_tx,
							me->comm[0].rx_rate = rate_rx );
				}

				// close file
				fprintf(logfile,"\n");
				fflush(logfile);
				fclose(logfile);
			}
		}
	}
}

